#ifndef COORDMATH_H
#define COORDMATH_H

#include <math.h>

/*! @brief WGS-84 elipsoid constant.
           Major axis length meters (center of ellipsoid to equator)
           used in conversions between ECEF and LLH */
#define C_WGS84_a 6378137.000

/*! @brief WGS-84 elipsoid constant.
           Minor axis length meters (center of ellipsoid to north/south pole)
           used in conversions between ECEF and LLH */
#define C_WGS84_b 6356752.31414  

/*!@brief Contains a point in ned.
 * @param north The distance north of the reference point in meters.
 * @param east  The distance east of the reference point in meters.
 * @param down  The distance from the center of the earth to the point (negitive).
 */
typedef struct {
    double north;
    double east;
    double down;
} NEDvector; /*end struct NEDvector*/

/*!@brief Contains a point in llh.
 * @param lat The latitude of the point in radians.
 * @param lon The longitude of the point in radians.
 * @param height The height of the point above the geoid in meters.
 */
typedef struct {
    double lat;
    double lon;
    double height;
} LLHvector; /*end struct LLHvector*/

/*!@brief Contains a point in ecef.
 * @param x The distance from the center of the earth along an axis through the prime-merridian in meters.
 * @param y The distance from the center of the earth along an axis through the equator perpindicular to x in meters.
 * @param z The distance from the center of the earth along an axis through the north pole in meters.
 */
typedef struct {
    double x;
    double y;
    double z;
} ECEFvector; /*end struct ECEFvector*/

/*!@brief converts from ECEF to NED
 * @param coord The coordinates to be converted in ECEF.
 * @param ref   The llh coordinates of the origin in the ned coordinate system. 
 *
 * Converts from ECEF to NED based on the tangent plane given by ref (in LLH radians).
 * ECEF should be given in meters, NED also in meters.
 */
NEDvector ECEFtoNED( ECEFvector coord,  LLHvector ref);

/*!@brief converts from NED to ECEF
 * @param coord The coordinates of the point to be converted in NED.
 * @param ref   The llh coordinates of the origin in the ned coordinate system.
 * 
 * Converts from NED to ECEF based on the tangent plane given by ref (in LLH radians).
 * Both should be given in meters.
 */
ECEFvector NEDtoECEF( NEDvector coord, LLHvector ref);

/*!@brief converts from ECEF to LLH
 * @param coord The coordinates of ECEF
 *
 * Converts from ECEF to LLH. ECEF should be given in meters.
 */
LLHvector ECEFtoLLH( ECEFvector coord);

/*!@brief converts from LLH to ECEF
 * @param coord The coordinates of the point to be conveted in LLH 
 *
 * Converts from LLH to ECEF. ECEF should be given in meters.
 */
ECEFvector LLHtoECEF( LLHvector coord); 

/*!@brief returns the NED vector between two LLH coordinates
 * @param coord1 The first point in LLH given in radians
 * @param coord2 The second point in LLH given in radians
 * @param ref    The LLH coordinate of the origin in the ned coordinate system
 *
 * Returns the NED vector (in meters) between two LLH coordinates (in radians) based on the
 * tangent plane given by ref (in LLH radians).
 */
NEDvector deltaLLH( LLHvector coord1, LLHvector coord2, LLHvector ref);

/*!@brief returns the NED vector between two ECEF coordinates
 * @param coord1 The first point in ECEF given in meters
 * @param coord2 The second point in ECEF given in meters
 * @param ref    The LLH coordinates of the origin in the ned coordinate system
 *
 * Returns the NED vector (in meters) between two ECEF coordinates (in meters) based on the
 * NED tangent plane given by ref (in LLH radians).
 */
NEDvector deltaECEF( ECEFvector coord1, ECEFvector coord2, LLHvector ref);

#endif
